#include <ros.h>
#include <std_msgs/Bool.h>

ros::NodeHandle nh;

std_msgs::Bool pushed_msg;
ros::Publisher pub_button("pushed",&pushed_msg);

const int button_pin = 7;
const int led_pin = 13;

bool last_reading;
long last_debounce_time=0;
long debounce_daley =50;
bool published = true;

void setup()
{
  nh.initNode();
  nh.advertise(pub_button);

  pinMode(led_pin,OUTPUT);
  pinMode(button_pin,OUTPUT);
  digitalWrite(button_pin,HIGH);

  last_reading = !digitalRead(button_pin);
}

void loop() 
{
  bool reading =!digitalRead(button_pin);
  if(last_reading != reading)
  {
    last_debounce_time = millis();
    published = false;
  }

  if(!published && (millis()-last_debounce_time) > debounce_daley)
  {
    digitalWrite(led_pin,reading);
    pushed_msg.data = reading;
    pub_button.publish(&pushed_msg);
    published = true;
  }

  last_reading = reading;
  nh.spinOnce();
}





